package Model.SurroundModel;

import java.util.ArrayList;

import java.math.BigDecimal;
import java.math.RoundingMode;

public class ShipSecond {
    public int num; //船只编号
    public double ax; //x方向加速度
    public double ay; //y方向加速度
    public double vx; //x方向前进速度
    public double vy; //y方向前进速度
    public double vxi; //控制器的x方向前进速度
    public double vyi; //控制器的y方向前进速度
    public double vxe; //控制器的x方向前进速度
    public double vye; //控制器的y方向前进速度
    public double x; //船的x坐标
    public double y; //船的y坐标 x,y用于x（t）
    public double xi; //控制器的x坐标
    public double yi; //控制器的y坐标
    public double xe; //控制器的x坐标
    public double ye; //控制器的y坐标
    public double xd; //用于vxi求导的中间量
    public double yd; //用于vyi求导的中间量
    public double xdi; //用于vxi求导的中间量
    public double ydi; //用于vyi求导的中间量
    public double xx;
    public double yy; // xx,yy用于计算p(t),p(t)是目标中心的位置
    public double φx = 0;
    public double φy = 0;//表示目标平均中心动态估计器的内部状态

    public ArrayList<ShipSecond> ships = new ArrayList<ShipSecond>(); //船队
    public static double t0;//用于检测目标移动的时间变量
    public static double k1 = ParameterSecond.k1;
    public static double k2 = ParameterSecond.k2;

    public ShipSecond() {
    }

    public ShipSecond(int num, double x, double y) {
        this.x = x;
        this.y = y;
        this.num = num;
    }

    //计算欧氏距离
    public static double distance(double x1, double y1, double x2, double y2) {
        return Math.sqrt(Math.pow((x1 - x2), 2) + Math.pow((y1 - y2), 2));
    }

    //初始化智能体位置
    public void setPosition() {
        ships.add(new ShipSecond(1, -2, 3));
        ships.add(new ShipSecond(2, 2, 3));

        ships.add(new ShipSecond(4, 2, -3));
        ships.add(new ShipSecond(3, -3, -2));
    }

    //  接下来进行p（t）和 φ（t） 的计算
    public void setP(double t) {
        for (ShipSecond ship : ships) {
            ship.xx = TargetSecond.targets.get(ship.num - 1).x + ship.φx;
            ship.yy = TargetSecond.targets.get(ship.num - 1).y + ship.φy;
            BigDecimal B = new BigDecimal(ship.xx);
            ship.xx = B.setScale(3, RoundingMode.HALF_UP).doubleValue();
            BigDecimal C = new BigDecimal(ship.yy);
            ship.yy = C.setScale(3, RoundingMode.HALF_UP).doubleValue();
            TargetSecond.targets.get(ship.num - 1).xx = ship.xx;
            TargetSecond.targets.get(ship.num - 1).yy = ship.yy;

        }
    }

    public void setφ(double t) {
        for (ShipSecond ship : ships) {
            double φ1 = 0;
            double φ2 = 0;
            for (ShipSecond shipx : ships) {
                φ1 = φ1 + Math.signum(shipx.xx - ship.xx);
                φ2 = φ2 + Math.signum(shipx.yy - ship.yy);
            }
            φ1 = ParameterSecond.α * t * φ1;
            φ2 = ParameterSecond.α * t * φ2;
            ship.φx = φ1 + ship.φx;
            ship.φy = φ2 + ship.φy;
        }
    }

    //改变智能体的速度
    public void setv(double t) {
        for (ShipSecond ship : ships) {
            ship.vx += ship.ax * t;
            ship.vy += ship.ay * t;
        }
    }

    //改变智能体的状态信息
    public void setx(double t) {
        for (ShipSecond ship : ships) {
            ship.x += ship.vx * t;
            ship.y += ship.vy * t;
        }
    }

    public void setvi(double t) {
        double dt = 0.00000001;
        int i = 0;

        setP(t);
        for (ShipSecond ship : ships) {
            i = ship.num - 1;
            ship.xd = ship.xx + k1 * (TargetSecond.targets.get(i).x - ship.xx);
            ship.yd = ship.yy + k1 * (TargetSecond.targets.get(i).y - ship.yy);
        }

        TargetSecond.setPosition(ShipSecond.t0 + dt);
        TargetSecond.setCenter();
        setP(t + dt);
        for (ShipSecond ship : ships) {
            i = ship.num - 1;
            ship.xdi = ship.xx + k1 * (TargetSecond.targets.get(i).x - ship.xx);
            ship.ydi = ship.yy + k1 * (TargetSecond.targets.get(i).y - ship.yy);
        }

        for (ShipSecond ship : ships) {
            ship.vxi = (ship.xdi - ship.xd) / dt;
            ship.vyi = (ship.ydi - ship.yd) / dt;
        }
    }

    public void setxi(double t) {
        int i = 0;
        setP(t);
        for (ShipSecond ship : ships) {
            i = ship.num - 1;
            ship.xi = ship.xx + k1 * (TargetSecond.targets.get(i).x - ship.xx);
            ship.yi = ship.yy + k1 * (TargetSecond.targets.get(i).y - ship.yy);
        }
    }

    public void setxe(double t) {
        setx(t);
        setxi(t);
        for (ShipSecond ship : ships) {
            ship.xe = ship.x - ship.xi;
            ship.ye = ship.y - ship.yi;
        }
    }

    public void setve(double t) {
        setv(t);
        //ShipSecond.setvi(t);
        for (ShipSecond ship : ships) {
            ship.vxe = ship.vx - ship.vxi;
            ship.vye = ship.vy - ship.vyi;
        }
    }

    //二阶船队控制器
    public void cotroller(double t) {
        double x = 0;
        double y = 0;
        int i = 0;
        int j = 0;

        //获取xe，ye，vxe，vye的值
        setve(t);
        setxe(t);

        //进行控制器算法运算，计算下一时刻的加速度值
        for (ShipSecond ship : ships) {
            i = ship.num - 1;
            //System.out.println("num:" + ship.num + "  (xi,yi):(" + ship.xi + "," + ship.yi + ")");
            //System.out.println("num:" + ship.num + "  (vxi,vyi):(" + ship.vxi + "," + ship.vyi + ")");
            //System.out.println("num:" + ship.num + "  (xe,ye):(" + ship.xe + "," + ship.ye + ")");
            for (ShipSecond shipx : ships) {
                j = shipx.num - 1;
                x += ParameterSecond.A[i][j] * ((shipx.xe - ship.xe) + (shipx.vxe - ship.vxe));
                y += ParameterSecond.A[i][j] * ((shipx.ye - ship.ye) + (shipx.vye - ship.vye));
            }
            System.out.println("第"+i+"船的x，y");
            System.out.println(ship.x);
            System.out.println(ship.y);
            System.out.println("----------------");
            //ship.ax = -ship.xe - (k2 + 1) * ship.vxe + x;
            //ship.ay = -ship.ye - (k2 + 1) * ship.vye + y;
//            ship.ax = -ship.xe - ship.vxe + k2 * (ship.xx - ship.vx);
//            ship.ay = -ship.ye - ship.vye + k2 * (ship.yy - ship.vy);
            ship.ax = -ship.xe - ship.vxe + k2 * ( - ship.vx)+t*x;
            ship.ay = -ship.ye - ship.vye + k2 * ( - ship.vy)+t*y;
            //System.out.println("num:" + ship.num + "  (ax,ay):(" + ship.ax + "," + ship.ay + ")");
            //System.out.println("num:" + ship.num + "  (vx,vy):(" + ship.vx + "," + ship.vy + ")");
            x = 0;
            y = 0;
        }

    }
}
